Rpp interpolation #2872
Rpp interpolation #2872
Conversation
There was a problem hiding this comment.
LGTM, took me a hot minute and a diagram to get what you were going for, but I understand. Might be worth adding a few comments inline to make it more clear to other users about how this is being modeled. Particularly w.r.t. the 1.0 and -1.0 solutions and how this is setup. Might even be worth adding a link to the comment I left with the diagram so that users looking at the code later have a geometric look at it available
nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp
Outdated
Show resolved
Hide resolved
nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp
Outdated
Show resolved
Hide resolved
nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp
Outdated
Show resolved
Hide resolved
| @@ -372,6 +378,45 @@ void RegulatedPurePursuitController::rotateToHeading( | |||
| angular_vel = std::clamp(angular_vel, min_feasible_angular_speed, max_feasible_angular_speed); | |||
| } | |||
|
|
|||
| geometry_msgs::msg::Point circleSegmentIntersection( | |||
There was a problem hiding this comment.
A diagram would be helpful here as to how this interpolation is happening, but I think I understand the thinking in the diagram I attached below if that's the same as yours.
You have pt i and pt i-1 that you're trying to find the exact intersection point between on the circle of the lookahead radius (pt E). You set Pt i and Pt i-1 as 2 points on a line segment and use the math associated with circle-to-line intersection points to find Pt E.
The couple of assumptions you make is that the line segment roughly approximates the curve of the path. For small distances, that's a good assumption. For further ones in sparse paths, I do wonder if we shouldn't do something more. But if you find that this is good enough, that's fine enough for me. Its basically linear interpolation.
I think there's probably an easier way to do this with just linear interpolation on the line segment, but this works.
There was a problem hiding this comment.
Comment is mostly here for posterity
There was a problem hiding this comment.
Yes, I can add more documentation and clarity to the code. I just wanted to get this up here before I invested into that 😄 . Yes, you are understanding how it works correctly. As far as whether linear interpolation is good enough vs something more complex, it is good enough to solve my use-case, and it is an incremental improvement for the controller, so I think it is good enough. Beyond linear interpolation I think starts to get into the realm of path planning, but I think we should hold off trying to implement it until it is determined that there is a need.
- Add more documentation/comments about interpolation method
nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp
Outdated
Show resolved
Hide resolved
|
I've added some more unit tests to the interpolation method, and it looks like I caught some edge cases that I need to fix.
|
|
I see you removed the negative solution space, is it true that its always the positive solution, even if the path does a U-turn and the "real" solution is behind the robot pose? |
While I will verify this with making sure the unit tests pass, I believe it is possible to make the closed form only and always return the intersecting point that is in between the two points. For reference, here is the graph I have been experimenting with for these different formulas, which includes one formula I derived on my own, and one formula used in the page that I linked to in my comment (which is what is implemented in the code now). https://www.desmos.com/calculator/23wsuzzfwo |
|
Ok, I think I captured the correct solution now, here: https://www.desmos.com/calculator/xhzoq4wq0r |
|
That min/max check you had before seemed rational and doesn't take any significant compute time. Even if you don't find the opposite solution, you could at least reject the update to use the raw lookahead point vs. returning a totally wrong-direction-ed point. Though, that geometry tool you showed is illustrative (you have a bunch of these cool web tools in your pocket!) I think you're probably correct that that solution is generally true. Maybe I'm being overly cautious / concerned if there's a bug later down the line creating a subtle issue. Maybe I'm just being paranoid 😆 I leave it up to you and how confident you are that this will not be a problem in the future. |
|
I believe I have addressed all comments. All unit tests are now passing, and I have added an interactive jupyter notebook to the docs folder to illustrate the interpolation algorithm, as well as additional comments. I also have opened the corresponding docs PR to add the parameter |
|
Some reason CI jobs have been a little flaky to trigger, just toggled to get CI to run |
nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp
Outdated
Show resolved
Hide resolved
* add lookahead interpolation test * add use_lookahead_point_interpolation parameter * initial implementation for lookahead interpolation * fix lookahead interpolation unit test * add use_lookahead_point_interpolation parameter to README * replace std::pow with multiplication * use double rather than auto * simplify circleSegmentIntersection * convert circleSegmentIntersection to method * added more unit tests for circleSegmentIntersection * fix circleSegmentIntersection to return correct intersection * added interactive jupyter notebook to document interpolation formula * black autoformat intersection notebook * more comments on rpp interpolation * added more unit tests for circleSegmentIntersection * rename use_lookahead_point_interpolation to use_interpolation * enable rpp interpolation by default * fix circle-segment-intersection notebook * formatting
Basic Info
Description of contribution in a few bullet points
Adds parameter
use_lookahead_point_interpolationthat interpolates between poses on path to ensure the lookahead point is exactly the desired lookahead distance away from the robot. This leads to smoother commanded velocities for sparse paths.Description of documentation updates required from your changes
Add parameter docs.
Future work that may be required in bullet points
For Maintainers: